
/**
 * Delta X Firmware
 * Copyright (c) 2020 DeltaXFirmware [https://github.com/deltaxrobot/Delta-X-Firmware]
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
// Motion.h

#ifndef _MOTION_h
#define _MOTION_h

#if defined(ARDUINO) && ARDUINO >= 100
#include "arduino.h"
#else

#include "WProgram.h"

#endif

#include "Constants.h"
#include "Tool.h"
#include "DeltaKinematics.h"
#include "Planner.h"
#include "Stepper.h"
#include "MultiServo.h"
//#include "GCodeReceiver.h"

/**
 * 各种动作指令的管理类
 */
class MotionClass {
protected:


public:
    void init();

    void G0(float xPos, float yPos, float zPos, float ewPos);

    void G1(float xPos, float yPos, float zPos, float ewPos);

    void G2(float i, float j, float xPos, float yPos, float wPos);

    void G3(float i, float j, float xPos, float yPos, float wPos);

    void G4(float p);//Delay ms
    void G5(float i, float j, float p, float q, float x, float y, float wPos);

    void G6(float angle1, float angle2, float angle3, float distance);

    void G28();//Homing
private:
    uint16_t NumberSegment;

    /**
     * 线性插值
     * 大意为：从Data.CurrentPoint 到 Data.DesiredPoint这两个点，按照线性关系，把亮点的直线距离按照 Data.MMPerLinearSegment(每段距离的长度，default：12mm)，分割成多段，并且发送到Planer计划执行队列。
     * 段数总量为：(Data.DesiredPoint - Data.CurrentPoint) / Data.MMPerLinearSegment
     * @return 线性插值是否成功
     */
    bool LinearInterpolation();

    /**
     * 按照圆弧进行插值
     * @param i
     * @param j
     * @param clockwise
     * @return
     */
    bool CircleInterpolation(float i, float j, bool clockwise);

    /**
     * 未实现
     * @param r
     * @param clockwise
     * @return
     */
    bool CircleInterpolation(float r, bool clockwise);

    /**
     * 贝塞尔曲线三次采样插值，只在G5命令中使用，G5专指此移动方式，默认会把所有的运动无脑分成30段
     * @param p1
     * @param p2
     * @return
     */
    bool Bezier4PointInterpolation(Point p1, Point p2);

    /**
     * 发送到执行计划
     * @param angle1 当前角度（包括所有的：theta1、theta2、theta3）
     * @param angle2 下一个点的角度
     * @param distance 预期执行的距离
     * @param index 段的序号
     */
    void UploadSegment(Angle angle1, Angle angle2, float distance, uint8_t index);

    void SetHomePosition();
};

/**
 * 声明一个动作管理器
 */
extern MotionClass Motion;

#endif

